#ifndef MT6701_SENSOR_H
#define MT6701_SENSOR_H

#include "main.h"
#include <cstdint>

#define _2PI 6.28318530718f
#define _PI  3.14159265359f

struct SensorData {
    float angle = 0.0f;
    float velocity = 0.0f;
    uint8_t step = 1;
};

class MT6701Sensor {
public:
    // 获取实例（静态）
    static MT6701Sensor& getInstance();  // 返回引用

    // 构造函数
    MT6701Sensor(SPI_HandleTypeDef* spi, GPIO_TypeDef* csPort, uint16_t csPin, uint8_t polePairs);

    // SPI通信
    void csEnable();
    void csDisable();
    uint16_t getRawData();

    // 获取原始角度（弧度）
    float getRawAngle();

    // 获取累计多圈角度（弧度）
    float getFullAngle();

    // 获取速度（单位：rps）
    float getVelocity();

    // 获取电角度（归一化 0~2PI）
    float getElectricalAngle();

    // 初始化角度（例如在校准零点时调用）
    void initFirstAng(float angle);

    void update();

    // 获取当前六步换相的扇区编号（1~6）
    uint8_t getCommutationStep();

    // 获取带滤波后的扇区编号（防抖）
    uint8_t getFilteredStep();

    // 设置电角度零偏
    void setZeroOffset(float offset) { _zeroElectricalOffset = offset; }

    // 公共传感器数据接口
    SensorData sensorData;

private:
    // SPI 句柄与片选引脚
    SPI_HandleTypeDef* _spi;
    GPIO_TypeDef* _csPort;
    uint16_t _csPin;

    // 电机参数
    uint8_t _polePairs;

    // 角度跟踪相关
    bool _isFirstCall = true;
    float _angleLast = 0.0f;
    float _fullAngleLast = 0.0f;
    int _rotationCount = 0;
    int _rotationCountLast = 0;
    float _zeroElectricalOffset = 0.0f;

    // 工具方法
    float _normalizeAngle(float angle);

    static MT6701Sensor _instance;       // 单例对象
};

#endif // MT6701_SENSOR_H
